package robotics.robots;

import InterpretadorV2.InterpretadorV2;
import java.rmi.RemoteException;
import robotics.interpretador.IInterpretador;
import robotics.interpretador.InterpretadorLocal;

public class GeometryRobot implements IRobot {

    private boolean debug = false;
    private final double wheelRadius = 2.8; // cm
    private final double dbw = 9.5; // cm
    private final int CALCULATE = 1;
    private final int READ = 2;
    private final int VERIFY = 3;
    private final int STRAIGHT_OFFSET = 20;// Minimum needed distance to move and error offset
    private double id = 0;
    double factor; // factor between left and right wheel in curve move
    double radiusMax;
    double radiusMin;
    double speedMin;
    double speedMax;
    double distanceMax;
    double distanceMin;
    double distance;
    private double pd = 0;
    private double degrees;
    final int speedRobot = 50;
    double offsetr = 0;
    double offsetl = 0;
    private boolean stopped = true;
    IInterpretador interp;

    public GeometryRobot(IInterpretador interpretador) {
        this.interp = interpretador;
//        new InterpretadorV2().Go(READ, READ, READ, READ, READ, debug);
    }

    // <editor-fold desc="getters and setters" defaultstate="collapsed">
    public void setOffsetL(double offsetl) {
        this.offsetl = offsetl;
    }

    public void setOffsetR(double offsetr) {
        this.offsetr = offsetr;
    }
    // </editor-fold>

    /**
     * must be at least 20 cm
     *
     * @param distance in cm
     */
    @Override
    public void straight(double distance) throws RemoteException {
        if (Math.abs(distance) < STRAIGHT_OFFSET) {
            throw new IllegalArgumentException("distance must be > " + STRAIGHT_OFFSET);
        }
        distance -= (STRAIGHT_OFFSET * Math.signum(distance));

        int state = CALCULATE;
        stopped = false;
        while (!stopped) {
            switch (state) {
                case CALCULATE:
                    degrees = Calculate(distance);
                    int l = (int) offsetl;
                    int r = (int) offsetr;
                    if (distance >= 0) {
                        interp.OnFwd(interp.OUT_B, speedRobot - (int) l + (int) r,
                                interp.OUT_C, speedRobot - (int) r + (int) l);
                    } else {
                        interp.OnFwd(interp.OUT_B, -(speedRobot - (int) l + (int) r),
                                interp.OUT_C, -(speedRobot - (int) r + (int) l));
                    }
                    state = READ;
                    break;
                case READ:
                    id = interp.MotorRotationCount(interp.OUT_B);
                    System.out.println("id " + id);
                    state = VERIFY;
                    break;
                case VERIFY:
                    pd = interp.MotorRotationCount(interp.OUT_B);
                    if (Math.abs(pd - id) > degrees) {
                        System.out.println("degrees = " + degrees);
                        System.out.println("(pd - id) = " + (pd - id));
                        return;
                    }
                    break;
            }
        }
    }

    private double Calculate(double distance) {
        return ((Math.abs(distance) / wheelRadius) * (180 / Math.PI));
    }

    @Override
    public void curveLeft(double radius, double angle) throws RemoteException {
        distance -= (STRAIGHT_OFFSET * Math.signum(distance));
        int state = CALCULATE;
        int pd[] = {0, 0};
        int id[] = {0, 0};
        stopped = false;
        while (!stopped) {
            switch (state) {
                case CALCULATE:
                    if (CalculateCurveMove(radius, angle)) {
                        int l = (int) offsetl;
                        int r = (int) offsetr;
                        interp.OnFwd(interp.OUT_C, (int) (speedMin - l), interp.OUT_B, (int) (speedMax - r));
                        state = READ;
                        break;
                    } else {
                        System.out.println("Smth is wrong, robot will not move");
                        return;
                    }
                case READ:
                    id = interp.MotorRotationCount(interp.OUT_B, interp.OUT_C);
                    state = VERIFY;
                    break;
                case VERIFY:
                    pd = interp.MotorRotationCount(interp.OUT_B, interp.OUT_C);
                    if (((pd[0] + pd[1]) / 2 - (id[0] + id[1]) / 2)
                            > distance / wheelRadius * 180 / Math.PI) {
                        //here we obtain angle in degrees, from distance
                        return;
                    }
                    break;
            }
        }
    }

    @Override
    public void curveRight(double radius, double angle) throws RemoteException {
        distance -= (STRAIGHT_OFFSET * Math.signum(distance));
        int state = CALCULATE;
        int pd[] = {0, 0};
        int id[] = {0, 0};
        stopped = false;
        while (!stopped) {
            switch (state) {
                case CALCULATE:
                    if (CalculateCurveMove(radius, angle)) {
                        int l = (int) offsetl;
                        int r = (int) offsetr;
                        interp.OnFwd(interp.OUT_C, (int) (speedMax - l),
                                interp.OUT_B, (int) (speedMin - r));
                        state = READ;
                        break;
                    } else {
                        System.out.println("Smth is wrong, robot will not move");
                        return;
                    }
                case READ:
                    id = interp.MotorRotationCount(interp.OUT_B, interp.OUT_C);
                    state = VERIFY;
                    break;
                case VERIFY:
                    pd = interp.MotorRotationCount(interp.OUT_B, interp.OUT_C);
                    if (((pd[0] + pd[1]) / 2 - (id[0] + id[1]) / 2)
                            > distance / wheelRadius * 180 / Math.PI) {
                        // here we obtain angle in degrees, from distance
                        return;
                    }
                    break;
            }
        }
    }

    private boolean CalculateCurveMove(double radius, double angle) {
        if (radius <= dbw / 2 || angle < 25) {
            System.out.println("Radius or angle is too small");
            return false;
        } else {
            angle = Math.toRadians(angle);
            distance = radius * angle; //Perimiter in cm
            distanceMin = (radius - dbw / 2) * angle; //Perimeter of arc of the left wheel circle
            distanceMax = (radius + dbw / 2) * angle; //Perimeter of arc of the right wheel circle
            factor = distanceMax / distanceMin;
            speedMax = Math.floor((2 * factor) / (factor + 1) * speedRobot);
            speedMin = Math.floor(2 / (factor + 1) * speedRobot);
            factor = speedMax / speedMin;//recalculating factor after rounding speeds
            return true;
        }
    }

    /**
     * a,b,c coefficients of quadratic equation a(x*x)+bx+c=0 return maximum of
     * 2 solutions
     *
     * @param a
     * @param b
     * @param c
     * @return double
     */
    public Double solveQuadraticEquation(double a, double b, double c) {
        double[] solutions = null;
        double discriminant;
        discriminant = Math.pow(b, 2) - 4 * c * a;
        if (discriminant < 0) {
            System.out.println("Discriminant is negative = " + discriminant);
            return null;
        } else {
            solutions[0] = (-b + Math.sqrt(discriminant)) / (2 * a);
            solutions[1] = (-b - Math.sqrt(discriminant)) / (2 * a);
            return Math.max(solutions[0], solutions[1]);
        }
    }

    /**
     * Calculate first trajectory of the robot after
     *
     * Only when x > y
     *
     * !!!!!! return null if discriminant negative
     *
     * @param x x coordinate
     * @param y y coordinate
     * @param f the angle in radiance
     * @return double[]{radius,angle,distance,f-angle}
     */
    private double[] firstTrajectory(double x, double y, double f) {
        //TODO: need to check constraints from pdf file like x>y and others
        // where x > y

        double distance;//straight distance
        // a, b, c coefficients of square equation
        double a;
        double b;
        double c;

        Double r; //positive solution of quadratic equation
        double angle;//angle of curveLeft/curveRight move
        a = 2 + 2 * Math.cos(f);
        b = 2 * y * (1 - Math.cos(f)) + 2 * x * Math.sin(f);
        c = -(Math.pow(x, 2) + Math.pow(y, 2));
        r = solveQuadraticEquation(a, b, c);
        if (r == null) {
            System.out.println("Discriminant is negative");
            return null;
        } else {
            angle = Math.asin((x - r * Math.sin(f)) / 2 * r);
            if (CalculateCurveMove(r, angle)) {
                r = (dbw / 2) * ((factor + 1) / (factor - 1));// recalcs radius from (1 trajectory pdf)
                distance = Math.sqrt(Math.pow((-x + r * Math.sin(f)), 2) //from (1 trajectory pdf)
                        + Math.pow(r - (y + r * Math.cos(f)), 2));
                angle = Math.acos((x - r * Math.sin(f)) / distance);//angle of the first curve
                return new double[]{r, angle, distance, f - angle};
                //TODO: curveLeft(r,angle)
                //TODO: straight(distance)
                //TODO: curveLeft(r,f-angle)
            } else {
                return new double[]{};
            }
        }

//        angle = Math.asin((x - r * Math.sin(f)) / 2 * r);
    }

    /**
     * *
     * Calculate second trajectory of the robot
     *
     * Only when x > y, (45>f>90)
     *
     * !!!!!!! returns null if discriminant negative
     *
     * @param x x coordinate
     * @param y y coordinate
     * @param f the angle in radiance
     * @return double[]{r, alpha, alpha - f}
     */
    public double[] secondTrajectory(double x, double y, double f) {
        // a, b, c coefficients of square equation
        double a;
        double b;
        double c;
        double alpha; // angle of first curve. second curve alpha-f
        double factor;
        double minVelocity;
        double maxVelocity;
        double pointX;// x coordinate of second curves radius
        double pointY;// y coordinate of second curves radius
        double d12;//distance between two radius points
        double delta;//angle
        double distance_straight;//distance for straight movement
        Double r = null; // radius for both curves.
        a = 2 - 2 * Math.cos(f);
        b = 2 * y * (1 + Math.cos(f) - 2 * x * Math.sin(f));
        c = -(x * x + y * y);
        r = solveQuadraticEquation(a, b, c); // radius with error (Theoretical)
        if (r == null) {
            System.out.println("Discriminant is negative. Something wrong");
            return null;
        } else {
            factor = (r + dbw / 2) / (r - dbw / 2); // factor with error (Theoretical)
            minVelocity = 2 / (factor + 1) * speedRobot;
            maxVelocity = 2 * speedRobot - minVelocity;
            factor = maxVelocity / minVelocity;//factor without error (Theoretical)
            r = (dbw / 2) * ((factor + 1) / (factor - 1));
            pointX = x + r * Math.sin(f);
            pointY = y - r * Math.cos(f);
            d12 = Math.sqrt(pointX * pointX + (r - pointY) * (r - pointY));
            delta = Math.acos(r / (d12 / 2));
            distance_straight = d12 * Math.sin(delta);
            alpha = Math.asin(pointX / d12) - delta; // from pdf
            return new double[]{r, alpha, distance_straight, alpha - f};
        }
    }

    public void stop() throws RemoteException {
        stopped = true;
        interp.Float(interp.OUT_BC);
    }

    @Override
    public void setDebug(Boolean debug) {
        this.debug = debug;
    }

    @Override
    public void log(String str) {
        if (debug) {
            System.out.println(str);
        }
    }

    public static void main(String[] args) throws RemoteException {
        InterpretadorLocal il = new InterpretadorLocal();
        GeometryRobot rob = new GeometryRobot(il);
        if (il.openRobot("Guia4")) {
            System.out.println("Going to start");
            // rob.testOffset();
//            rob.straight(10);
//            rob.CurveLeft(190, 90);
//            rob.CurveRight(190, 90);
//            rob.straight(-200);
//            rob.curveLeft(30, 90);
            rob.stop();
            il.closeRobot();
        }
        System.exit(0);
    }
}
